/*
 * @Description  : Phase Follow Application Module
 * @Version      : 0.1
 * @Company      : V-Think Development Team
 * @Author       : KiraVerSace@yeah.net
 * @Date         : 2025-01-22 14:00:00
 */
#include <appPhaseFollow.h>
#include <cstdio>

// 静态内部变量
static RingBuf<uint8_t, 200> phaseRingBuffer;
static HardwareTimer phaseSampleTimer(PHASE_SAMPLE_TIMER);
static HardwareTimer phaseDelayTimer(PHASE_DELAY_TIMER);
static HardwareTimer *pwmMeasureTimer = nullptr;
static bool phaseOutputDelayFlag = false;
static uint32_t measureBaseClockFrequency = 0;

static volatile float frequencyMeasured = 0, dutyCycleMeasured = 0;
static volatile uint32_t freqCount = 0, lastFreqCount = 0;
static volatile uint32_t dutyCycleCount = 0;

static void formatFixedPoint(char *buffer, size_t length, float value, uint8_t decimals = 2)
{
    if (length == 0)
    {
        return;
    }

    int32_t scale = 1;
    for (uint8_t i = 0; i < decimals; ++i)
    {
        scale *= 10;
    }

    float scaledValue = value * static_cast<float>(scale);
    int32_t scaled = static_cast<int32_t>((scaledValue >= 0.0F) ? (scaledValue + 0.5F) : (scaledValue - 0.5F));
    int32_t integerPart = scaled / scale;
    int32_t fractionalPart = scaled % scale;
    if (fractionalPart < 0)
    {
        fractionalPart = -fractionalPart;
    }

    if (decimals == 0U)
    {
        (void)snprintf(buffer, length, "%ld", static_cast<long>(integerPart));
    }
    else
    {
        (void)snprintf(buffer, length, "%ld.%0*ld", static_cast<long>(integerPart), decimals, static_cast<long>(fractionalPart));
    }
}

void phaseFollowInit(void);
void phaseSampleTimerCallback(void);
void phaseDelayTimerCallback(void);
void pwmTimerCaptureRisingCallback(void);
void pwmTimerCaptureFallingCallback(void);

osThreadId phaseFollowTaskHandle;
void phaseFollowTask(void *pvParameters)
{
    UNUSED(pvParameters);
    xLog.info("phaseFollowTask...[%d]\r\n", uxTaskGetStackHighWaterMark(NULL));

    // 初始化Phase Follow模块
    phaseFollowInit();

    // 等待系统稳定和初始频率测量
    vTaskDelay(pdMS_TO_TICKS(1000));

    // 根据测量频率设置延迟时间
    if ((frequencyMeasured >= 45) && (frequencyMeasured <= 55))
    {
        phaseDelayTimer.setOverflow(10000);
        xLog.info("Phase=[50]Hz, Delay=[10]ms\r\n");
    }
    else if ((frequencyMeasured > 55) && (frequencyMeasured <= 65))
    {
        phaseDelayTimer.setOverflow(8330);
        xLog.info("Phase=[60]Hz, Delay=[8.33]ms\r\n");
    }
    else
    {
        phaseDelayTimer.setOverflow(10000);
        xLog.info("Phase=[Unknown], Delay=[10]ms(default)\r\n");
    }

    // 启动定时器
    phaseSampleTimer.resume();
    phaseDelayTimer.resume();

    xLog.info("Phase Follow started - measuring PWM frequency and dutyCycle...\r\n");

    while (1)
    {
        // 定期输出测量结果
        char frequencyBuffer[16] = {0};
        char dutyCycleBuffer[16] = {0};
        formatFixedPoint(frequencyBuffer, sizeof(frequencyBuffer), frequencyMeasured);
        formatFixedPoint(dutyCycleBuffer, sizeof(dutyCycleBuffer), dutyCycleMeasured);
        // xLog.trace("Frequency = %s Hz, DutyCycle = %s%%, RingBuffer Size = %d\r\n",
        //            frequencyBuffer, dutyCycleBuffer, phaseRingBuffer.size());

        // 检查频率变化，动态调整延迟时间
        static float lastFrequency = 0;
        if (fabs(frequencyMeasured - lastFrequency) > 5.0F) // 频率变化超过5Hz
        {
            if ((frequencyMeasured >= 45) && (frequencyMeasured <= 55))
            {
                phaseDelayTimer.pause();
                phaseDelayTimer.setOverflow(10000);
                phaseDelayTimer.resume();
                xLog.info("Frequency changed to 50Hz range, Delay=[10]ms\r\n");
            }
            else if ((frequencyMeasured > 55) && (frequencyMeasured <= 65))
            {
                phaseDelayTimer.pause();
                phaseDelayTimer.setOverflow(8330);
                phaseDelayTimer.resume();
                xLog.info("Frequency changed to 60Hz range, Delay=[8.33]ms\r\n");
            }
            lastFrequency = frequencyMeasured;
        }

        watchdogReport(WATCHDOG_SRC_PHASE_FOLLOW);

        // 每秒更新一次
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

void phaseFollowInit(void)
{
    // GPIO初始化
    pinMode(PHASE_IN1, INPUT);
    pinMode(PHASE_IN2, INPUT);
    pinMode(PHASE_IN3, INPUT);
    pinMode(PHASE_OUT1, OUTPUT);
    pinMode(PHASE_OUT2, OUTPUT);
    pinMode(PHASE_OUT3, OUTPUT);

    // 采样定时器初始化
    phaseSampleTimer.setPrescaleFactor(72);
    phaseSampleTimer.setOverflow(100);
    phaseSampleTimer.attachInterrupt(phaseSampleTimerCallback);

    // 延迟定时器初始化
    phaseDelayTimer.setPrescaleFactor(72);
    phaseDelayTimer.setOverflow(10000);
    phaseDelayTimer.attachInterrupt(phaseDelayTimerCallback);

    // PWM测量定时器初始化
    pwmMeasureTimer = new HardwareTimer(PHASE_MEASURE_TIMER);
    pwmMeasureTimer->setMode(1, TIMER_INPUT_FREQ_DUTY_MEASUREMENT, PHASE_IN1);
    /**
     * @brief 如果pwmTimer溢出,则说明测量的频率太低,将频率和占空比的值清零
     *        为了降低可测量最小频率,可以增加预分频器,会牺牲精度
     */
    pwmMeasureTimer->setPrescaleFactor(144);
    pwmMeasureTimer->setOverflow(0x10000);
    pwmMeasureTimer->attachInterrupt(1, pwmTimerCaptureRisingCallback);
    pwmMeasureTimer->attachInterrupt(2, pwmTimerCaptureFallingCallback);

    pwmMeasureTimer->resume();

    measureBaseClockFrequency = pwmMeasureTimer->getTimerClkFreq() / pwmMeasureTimer->getPrescaleFactor();

    xLog.info("PhaseFollow Init completed\r\n");
    xLog.info("InputCapture Timer=[TIM1] parameters:\r\n");
    char clockFreqBuffer[16] = {0};
    char measureBaseFreqBuffer[16] = {0};
    formatFixedPoint(clockFreqBuffer, sizeof(clockFreqBuffer), pwmMeasureTimer->getTimerClkFreq() / 1000000.0F);
    formatFixedPoint(measureBaseFreqBuffer, sizeof(measureBaseFreqBuffer), measureBaseClockFrequency / 1000.0F);
    xLog.info("ClockFrequency = [%s]MHz\r\n", clockFreqBuffer);
    xLog.info("PrescaleFactor = [%d]\r\n", pwmMeasureTimer->getPrescaleFactor());
    xLog.info("MeasureBaseClockFrequency = [%s]KHz\r\n", measureBaseFreqBuffer);
}

void phaseSampleTimerCallback(void)
{
    uint8_t pushData = 0;

    bitWrite(pushData, PHASE1_BIT_POSITION, digitalRead(PHASE_IN1));
    bitWrite(pushData, PHASE2_BIT_POSITION, digitalRead(PHASE_IN2));
    bitWrite(pushData, PHASE3_BIT_POSITION, digitalRead(PHASE_IN3));

    phaseRingBuffer.push(pushData);

    if (phaseOutputDelayFlag == true)
    {
        uint8_t popData = 0;

        if (phaseRingBuffer.pop(popData))
        {
            digitalWrite(PHASE_OUT1, bitRead(popData, PHASE1_BIT_POSITION));
            digitalWrite(PHASE_OUT2, bitRead(popData, PHASE2_BIT_POSITION));
            digitalWrite(PHASE_OUT3, bitRead(popData, PHASE3_BIT_POSITION));
        }
    }
}

void phaseDelayTimerCallback(void)
{
    phaseDelayTimer.pause();
    phaseOutputDelayFlag = true;
}

void pwmTimerCaptureRisingCallback(void)
{
    freqCount = pwmMeasureTimer->getCaptureCompare(1) + 1;

    /* Frequency computation */
    if (freqCount > lastFreqCount)
    {
        frequencyMeasured = (float)measureBaseClockFrequency / (freqCount - lastFreqCount);
        dutyCycleMeasured = (float)(dutyCycleCount * 100) / (freqCount - lastFreqCount);
    }
    else if (freqCount <= lastFreqCount) // 0x1000 溢出
    {
        frequencyMeasured = (float)measureBaseClockFrequency / (0x10000 + freqCount - lastFreqCount);
        dutyCycleMeasured = (float)(dutyCycleCount * 100) / (0x10000 + freqCount - lastFreqCount);
    }

    /* 由于加了光耦逻辑需要取反 */
    // dutyCycleMeasured = 100.0F - dutyCycleMeasured;  // 暂时取消光耦,变为正逻辑

    lastFreqCount = freqCount;
}

void pwmTimerCaptureFallingCallback(void)
{
    freqCount = pwmMeasureTimer->getCaptureCompare(2) + 1;

    /* DutyCycle computation */
    if (freqCount > lastFreqCount)
    {
        dutyCycleCount = freqCount - lastFreqCount;
    }
    else if (freqCount <= lastFreqCount)
    {
        dutyCycleCount = 0x10000 + freqCount - lastFreqCount;
    }
}
